
#ifndef _POSE_H_
#define _POSE_H_

#include <vector>
#include "../meshdef/cgaltypes.h"

#include <cpplapack.h>
typedef CPPL::dgematrix    MatrixCPPL;     ///< A CPPLapack matrix
typedef CPPL::dcovector    CoVectorCPPL;   ///< A CPPLapack column vector 

/// This represents the instantiation (Transformation) of a number
/// of rigid objects
typedef std::vector<Transformation> Pose;

/// A Frame is simply a pose to which a point in 3-space is associated
struct Frame 
{
    Point keyPoint;
    Pose pose;
};

/// A Frameset is a collection of frames that defines a 3D interpolation
/// scheme. 
typedef std::vector<Frame> FrameSet;

/// Given a frame set and a point in 3-space it is possible to interpolate
/// a new pose for all objects
class RbfPoseInterpolator 
{
    
    FrameSet& frames;
    std::vector<Point> center;
    int axes [3];
    unsigned naxes;
    unsigned n;
    //~ double *matrix, *inverseMatrix;
    //~ double *indep;
   
   MatrixCPPL _inverseMatrix;

public:
    /// Sets up the interpolation scheme for a set of frames fs.
    RbfPoseInterpolator (FrameSet& fs);

    /// Destructor
    ~RbfPoseInterpolator();

    /// Interpolates a new pose for a given keypoint. ComputeRbf must 
    /// have been called earlier.
    /// @param p position of new keypoint.
    /// @param pose pose at new keypoint.
    void interpolatePose (const Point& p, Pose& pose);
} ;  

#endif
